﻿using System;
using Microsoft.SPOT;
using TI_BLE_HCI_ClientLib.Controllers;
using System.Threading;
using TI_BLE_HCI_ClientLib.HCIEvents;
using TI_BLE_HCI_ClientLib.HCIBaseEntities;

namespace KeyFob_Tester_NETMF
{
    public static class Program
    {
        static byte[] KEYFOB_ADDR = { 0x96, 0x7d, 0x0b, 0xaf, 0x59, 0x90 };
        static byte[] SENSORTAG_ADDR = { 0x4e, 0x58, 0x6e, 0xe5, 0xc5, 0x78 }; // 78:C5:E5:6E:58:4E

        static HCISimpleSerialCentralProfileController _controller;

        public static void Main()
        {
            Debug.Print("KeyFob Tester Running...");

            if (!InitController()) return;

            //Test_AlertService();
            //Test_BatteryService();
            //Test_DeviceInfo();
            //Test_KeyPressedNotification();
            //Test_TwoFunctions();
            //Test_DrivenBySensorTag();
            //Test_TwoFunctions_Async();

            Test_SensorTag_GetTemperature();

            TerminateController();

            Debug.Print("...SUCCEEDED");
        }

        static bool InitController()
        {
            _controller = new HCISimpleSerialCentralProfileController(Mountaineer.Netmf.Hardware.Socket3.SerialPortName, 115200);

            _controller.Start();

            if (!_controller.Reset(HCIController.ResetTypeEnum.HardReset))
            {
                Debug.Print("Controller Reset FAILED!");
                return false;
            }

            if (!_controller.Init())
            {
                Debug.Print("Controller Init FAILED!");
                return false;
            }

            return true;
        }

        static void TerminateController()
        {
            _controller.Terminate();
        }

        static void Test_AlertService()
        {
            ushort connHandle;

            Debug.Assert(_controller.ConnectTo(KEYFOB_ADDR, out connHandle));

            Debug.Assert(_controller.WriteCharacteristic(connHandle, 0x0028, (ushort)1));

            Thread.Sleep(3000);

            Debug.Assert(_controller.WriteCharacteristic(connHandle, 0x0028, (ushort)2));

            Thread.Sleep(3000);

            Debug.Assert(_controller.WriteCharacteristic(connHandle, 0x0028, (ushort)0));

            Debug.Assert(_controller.CloseConnection(connHandle));
        }

        static void Test_BatteryService()
        {
            ushort connHandle;

            Debug.Assert(_controller.ConnectTo(KEYFOB_ADDR, out connHandle));

            byte value=0x00;

            Debug.Assert(_controller.ReadCharacteristicAsUInt8(connHandle, 0x002f, out value));

            Debug.Print("Battery Level = " + value + "%");

            Debug.Assert(value > 0);

            Debug.Assert(_controller.CloseConnection(connHandle));
        }

        static void Test_DeviceInfo()
        {
            ushort connHandle;

            Debug.Assert(_controller.ConnectTo(KEYFOB_ADDR, out connHandle));

            string value=null;

            Debug.Assert(_controller.ReadCharacteristicAsString(connHandle, 0x0003, out value));

            Debug.Print("Device Name = " + value);

            Debug.Assert(value == "TI BLE Keyfob");

            Debug.Assert(_controller.CloseConnection(connHandle));
        }

        static void Test_KeyPressedNotification()
        {
            ushort connHandle;

            Debug.Assert(_controller.ConnectTo(KEYFOB_ADDR, out connHandle));

            Debug.Assert(_controller.WriteCharacteristic(connHandle, 0x0048, (ushort)1));

            AutoResetEvent are = new AutoResetEvent(false);

            _controller.OnNotification += ((notification) =>
            {
                switch (notification.ValueAsUInt8)
                {
                    case 0x00:
                        Debug.Print("Key Released");
                        break;
                    case 0x01:
                        Debug.Print("Key 1 Pressed");
                        break;
                    case 0x02:
                        Debug.Print("Key 2 Pressed");
                        break;
                    case 0x03:
                        Debug.Print("Both Keys Pressed...exiting");
                        are.Set();
                        break;
                }

            });

            are.WaitOne();

            Debug.Assert(_controller.WriteCharacteristic(connHandle, 0x0048, (ushort)0));

            Debug.Assert(_controller.CloseConnection(connHandle));
        }

        static void Test_TwoFunctions()
        {
            ushort connHandle=0xFFFF;

            Debug.Assert(_controller.ConnectTo(KEYFOB_ADDR, out connHandle));

            Debug.Assert(_controller.WriteCharacteristic(connHandle, 0x0048, (ushort)1));

            AutoResetEvent are = new AutoResetEvent(false);

            _controller.OnNotification += ((notification) =>
            {
                if (notification.ValueAsUInt8 == 3)
                {
                    Debug.Print("Exiting...");
                    are.Set();
                }
                else
                {
                    Debug.Print("Key State = " + notification.ValueAsUInt8);
                    _controller.WriteCharacteristic(connHandle, 0x0028, notification.ValueAsUInt8);
                }

            });

            are.WaitOne();

            Debug.Assert(_controller.WriteCharacteristic(connHandle, 0x0048, (ushort)0));

            Debug.Assert(_controller.CloseConnection(connHandle));
        }

        static void Test_DrivenBySensorTag()
        {
            ushort connHandleKeyfob;
            ushort connHandleSensorTag = 0xFFFF;

            Debug.Assert(_controller.ConnectTo(KEYFOB_ADDR, out connHandleKeyfob));
            Debug.Assert(_controller.ConnectTo(SENSORTAG_ADDR, out connHandleSensorTag));

            Debug.Assert(_controller.WriteCharacteristic(connHandleSensorTag, 0x006c, (ushort)1));

            AutoResetEvent are = new AutoResetEvent(false);

            _controller.OnNotification += ((notification) =>
            {
                if (notification.ConnHandle == connHandleSensorTag)
                {
                    Debug.Print("SensorTag Key State = " + notification.ValueAsUInt8);

                    switch (notification.ValueAsUInt8)
                    {
                        case 0x00:
                            Debug.Assert(_controller.WriteCharacteristic(connHandleKeyfob, 0x0028, (byte)0));
                            break;

                        case 0x01:
                            Debug.Assert(_controller.WriteCharacteristic(connHandleKeyfob, 0x0028, (byte)1));
                            break;

                        case 0x02:
                            Debug.Assert(_controller.WriteCharacteristic(connHandleKeyfob, 0x0028, (byte)2));
                            break;

                        case 0x03:
                            Debug.Print("Exiting...");
                            are.Set();
                            break;
                    }
                }
            });

            are.WaitOne();

            Debug.Assert(_controller.WriteCharacteristic(connHandleSensorTag, 0x006c, (ushort)0));

            Debug.Assert(_controller.CloseConnection(connHandleKeyfob));
            Debug.Assert(_controller.CloseConnection(connHandleSensorTag));
        }

        static void Test_TwoFunctions_Async()
        {
            ushort connHandle = 0xFFFF;

            Debug.Assert(_controller.ConnectTo(KEYFOB_ADDR, out connHandle));

            Debug.Assert(_controller.WriteCharacteristic(connHandle, 0x0048, (ushort)1));

            AutoResetEvent are = new AutoResetEvent(false);

            _controller.OnNotification += ((notification) =>
            {
                if (notification.ValueAsUInt8 == 3)
                {
                    Debug.Print("Exiting...");
                    are.Set();
                }
                else
                {
                    Debug.Print("Key State = " + notification.ValueAsUInt8);

                    Debug.Print("Writing characteristic...");

                    _controller.WriteCharacteristicAsync(connHandle, 0x0028, notification.ValueAsUInt8);

                    AutoResetEvent are2 = new AutoResetEvent(false);

                    HCIEventHandler handler = ((evt) =>
                    {
                        if (evt is HCIEvent_ATT_Write_Characteristic_Value_Response &&
                            (evt as HCIEvent_ATT_Write_Characteristic_Value_Response).ConnHandle == connHandle &&
                            (evt as HCIEvent_ATT_Write_Characteristic_Value_Response).Status == HCIVendorSpecificEventHostErrorCode.Success)
                        {
                            are2.Set();
                        }
                    });

                    _controller.HCIEventReceived += handler;

                    are2.WaitOne();

                    _controller.HCIEventReceived -= handler;

                    Debug.Print("...OK");
                }

            });

            are.WaitOne();

            Debug.Assert(_controller.WriteCharacteristic(connHandle, 0x0048, (ushort)0));

            Debug.Assert(_controller.CloseConnection(connHandle));
        }

        static void Test_SensorTag_GetTemperature()
        {
            ushort connHandle;

            Debug.Assert(_controller.ConnectTo(SENSORTAG_ADDR, out connHandle));

            /*var attributeErrorCode=AttributeErrorCodeEnum.None;
            Debug.Assert(!_controller.WriteCharacteristic(connHandle, 0x0029, 1,out attributeErrorCode));
            Debug.Assert(attributeErrorCode != AttributeErrorCodeEnum.None);*/

            Debug.Assert(_controller.WriteCharacteristic(connHandle, 0x0029, (byte)1));

            Thread.Sleep(1000);

            byte[] irtempdata=null;

            Debug.Assert(_controller.ReadCharacteristic(connHandle, 0x0025, out irtempdata));

            double tDie=CalculateAmbientTemperature(irtempdata);

            Debug.Print("Ambient Temperature = " + tDie);
            Debug.Print("Target Temperature = " + CalculateTargetTemperature(irtempdata,tDie));

            Debug.Assert(_controller.WriteCharacteristic(connHandle, 0x0029, /*(byte)*/0));

            Debug.Assert(_controller.CloseConnection(connHandle));
        }

        /// <summary>
        /// Calculates the ambient temperature.
        /// </summary>
        /// <param name="sensorData">Complete array of data retrieved from the sensor</param>
        /// <param name="scale"></param>
        /// <returns>Ambient temperature as double</returns>
        public static double CalculateAmbientTemperature(byte[] sensorData)
        {
            return BitConverter.ToUInt16(sensorData, 2) / 128.0;
        }

        /// <summary>
        /// Calculates the target temperature of the sensor.
        /// More info about the calculation: http://www.ti.com/lit/ug/sbou107/sbou107.pdf
        /// </summary>
        /// <param name="sensorData"></param>
        /// <param name="ambientTemperature"></param>
        /// <returns></returns>
        private static double CalculateTargetTemperature(byte[] sensorData, double ambientTemperature)
        {
            double Vobj2 = BitConverter.ToInt16(sensorData, 0);
            Vobj2 *= 0.00000015625;

            double Tdie = ambientTemperature + 273.15;

            double S0 = 5.593E-14;
            double a1 = 1.75E-3;
            double a2 = -1.678E-5;
            double b0 = -2.94E-5;
            double b1 = -5.7E-7;
            double b2 = 4.63E-9;
            double c2 = 13.4;
            double Tref = 298.15;
            double S = S0 * (1 + a1 * (Tdie - Tref) + a2 * System.Math.Pow((Tdie - Tref), 2));
            double Vos = b0 + b1 * (Tdie - Tref) + b2 * System.Math.Pow((Tdie - Tref), 2);
            double fObj = (Vobj2 - Vos) + c2 * System.Math.Pow((Vobj2 - Vos), 2);
            double tObj = System.Math.Pow(System.Math.Pow(Tdie, 4) + (fObj / S), .25);

            return tObj - 273.15;
        }
    }
}
